#include "StdAfx.h"
#include "WritingTrajectoire.h"
 
namespace Tools
{
	float WritingTrajectoire::alpha_thr0 = 0.3;
	float WritingTrajectoire::alpha_thr1 = 0.5;

	WritingTrajectoire::WritingTrajectoire(vector<CvPoint> writing_trajectoire, CvPoint p0, CvPoint p1)
	{
		this->spwt = writing_trajectoire;
		this->T0 = p0;
		this->T1 = p1;
		
		this->g = 0;
		this->alpha = 0;
		this->w = 0;
		
		this->calculate();
	}

	float WritingTrajectoire::get_direction_angle(CvPoint p0, CvPoint p1)
	{
		return (float) atan2((p1.y - p0.y) * 1.0 , (p1.x - p0.x) * 1.0);
	}
	
	float WritingTrajectoire::get_angle_difference(float angle0, float angle1)
	{
		return min((float)abs(angle0 - angle1), (float)(2 * M_PI - abs(angle0 - angle1)));
	}


	void WritingTrajectoire::calculate()
	{
		
		float start_angle = WritingTrajectoire::get_direction_angle(this->spwt[0], this->spwt[1]);
		for (int k = this->spwt.size() - 1; k > 0; k--)
		{
			bool condition1 = true;
			bool condition2 = true;
			float mean_angle = this->mean_angle(k);
			for (int i = 1; i <= k; i++)
			{
				float direction_angle = WritingTrajectoire::get_direction_angle(this->spwt[i-1], this->spwt[i]);
				float angle_difference = WritingTrajectoire::get_angle_difference(direction_angle,start_angle);
				float mean_difference = WritingTrajectoire::get_angle_difference(direction_angle,mean_angle);
				if (mean_difference > WritingTrajectoire::alpha_thr0)
				{
					condition1 = false;
					break;
				}
				if (angle_difference > WritingTrajectoire::alpha_thr1)
				{
					condition2 = false;
					break;
				}
			}
			if (condition1 && condition2)
			{
				this->g = k;
				this->alpha = mean_angle;
				break;
			}
		}

		float d = (float) Tools::distance_Euclidean(this->T0,this->T1);
		float angle = WritingTrajectoire::get_direction_angle(T0,T1) - this->alpha;
		this->w = d * abs(sin(angle));

		return;
	}

	float WritingTrajectoire::mean_angle(int L)
	{
		if ((L <= 0) || (L > (int) this->spwt.size() - 1))
		{
			return -1000;
		}
		float retval = 0.0;
		for (int i = 1; i <= L; i++)
		{
			float direction_angle = WritingTrajectoire::get_direction_angle(this->spwt[i-1], this->spwt[i]);
			retval+= direction_angle;
		}
		retval = retval / L;
		return retval;
	}

	float WritingTrajectoire::get_angle_deviation(const WritingTrajectoire &vj)
	{
		float retval;
		float beta = 0.0;
		beta = WritingTrajectoire::get_direction_angle(vj.spwt[0], this->spwt[0]);
		retval = abs(this->alpha - beta);// + abs(M_PI - abs(this->alpha - beta));
		return retval;
	}

	CvPoint WritingTrajectoire::get_chain_point(int k,const WritingTrajectoire &spwt_i, const WritingTrajectoire &spwt_j)
	{
		if (k >= 0)
		{
			return spwt_i.spwt[k];
		}
		else
		{
			return spwt_j.spwt[-1 * (k + 1)];
		}
	}

	float WritingTrajectoire::get_curvature(int k, const WritingTrajectoire &spwt_i, const WritingTrajectoire &spwt_j)
	{
		float retval = 0.0;
		
		CvPoint p1 = WritingTrajectoire::get_chain_point(k-1,spwt_i,spwt_j);
		CvPoint p2 = WritingTrajectoire::get_chain_point(k,spwt_i,spwt_j);
		CvPoint p3 = WritingTrajectoire::get_chain_point(k+1,spwt_i,spwt_j);

		CvPoint c1 = cvPoint((p1.x + p2.x) / 2, (p1.y + p2.y) / 2);
		CvPoint c2 = cvPoint((p2.x + p3.x) / 2, (p2.y + p3.y) / 2);
		double d = Tools::distance_Euclidean(c1,c2);

		float angle1 = WritingTrajectoire::get_direction_angle(p1,p2);
		float angle2 = WritingTrajectoire::get_direction_angle(p2,p3);

		float e = min((float)abs(angle1 - angle2), (float)(2 * M_PI - abs(angle1 - angle2)));
		if (d > 0)
		{
			retval = e / (float) d;
		}
		return retval;
	}

	float WritingTrajectoire::get_curvature_variation(const WritingTrajectoire &spwt_i, const WritingTrajectoire &spwt_j)
	{
		float retval = 0.0;
		for (int k = -spwt_j.g; k <= spwt_i.g - 2; k++)
		{
			float diff_curvature = abs(WritingTrajectoire::get_curvature(k,spwt_i,spwt_j) - WritingTrajectoire::get_curvature(k+1,spwt_i,spwt_j));
			CvPoint p1 = WritingTrajectoire::get_chain_point(k,spwt_i,spwt_j);
			CvPoint p2 = WritingTrajectoire::get_chain_point(k+1,spwt_i,spwt_j);
			double d = Tools::distance_Euclidean(p1,p2);
			if (d > 0)
			{
				retval+= diff_curvature / (float)d;
			}

		}
		retval = retval / (spwt_i.g + spwt_j.g - 1);
		return retval;
	}

	WritingTrajectoire::~WritingTrajectoire(void)
	{
	}

}
